#!/usr/bin/env python

import os
import sys
import rospy
import rospkg
import signal
from python_qt_binding import loadUi
from python_qt_binding.QtGui import QIcon
from python_qt_binding.QtWidgets import *
from interbotix_perception_modules.pointcloud import InterbotixPointCloudInterface

class GUI(QWidget):
    """Placeholder QWidget"""

class PointCloudTunerGui(QWidget):
    """GUI to tune the PointCloud Filtering parameters"""
    def __init__(self):
        super(PointCloudTunerGui, self).__init__()
        r = rospkg.RosPack()
        # try to locate and load the Qt ui file
        try:
            self.pkg_path = r.get_path("interbotix_perception_modules")
        except rospkg.ResourceNotFound as err:
            rospy.logerr((
                "[pointcloudtunergui] Could not find the package "
                    "'interbotix_perception_modules'.\n"
                "[pointcloudtunergui] Error was '%s'." % err))
            exit(1)
        loadUi(
            os.path.join(
                self.pkg_path,
                "src",
                "ui",
                "pointcloudtunergui.ui"
            ),
            self,
            {"GUI": GUI}
        )
        self.name_map = {}
        filter_ns = rospy.get_namespace().strip("/")
        self.pc_obj = InterbotixPointCloudInterface(
            filter_ns=filter_ns,
            init_node=False)
        self.filepath = self.pc_obj.get_filepath()
        self.setWindowIcon(
            QIcon(
                self.pkg_path + "/images/gui/icon/Interbotix_Circle.png"
            )
        )
        self.create_crop_box_block()
        self.create_voxel_grid_block()
        self.create_seg_block()
        self.create_ror_block()
        self.create_cluster_block()
        self.create_button_block()
        self.show()

    def create_crop_box_block(self):
        """Creates the Crop Box Filter Block"""
        self.create_sub_component(
            "X min [m]", self.pc_obj.set_x_filter_min,
            self.pc_obj.get_x_filter_min, -0.5, 0.5,
            self.doublespinbox_cropbox_xmin,
            self.hslider_cropbox_xmin, 2)
        self.create_sub_component(
            "X max [m]", self.pc_obj.set_x_filter_max,
            self.pc_obj.get_x_filter_max,-0.5, 0.5,
            self.doublespinbox_cropbox_xmax,
            self.hslider_cropbox_xmax, 2)
        self.create_sub_component(
            "Y min [m]", self.pc_obj.set_y_filter_min,
            self.pc_obj.get_y_filter_min, -0.5, 0.5,
            self.doublespinbox_cropbox_ymin,
            self.hslider_cropbox_ymin, 2)
        self.create_sub_component(
            "Y max [m]", self.pc_obj.set_y_filter_max,
            self.pc_obj.get_y_filter_max, -0.5, 0.5,
            self.doublespinbox_cropbox_ymax,
            self.hslider_cropbox_ymax, 2)
        self.create_sub_component(
            "Z min [m]", self.pc_obj.set_z_filter_min,
            self.pc_obj.get_z_filter_min,0.2, 1.5,
            self.doublespinbox_cropbox_zmin,
            self.hslider_cropbox_zmin, 2)
        self.create_sub_component(
            "Z max [m]", self.pc_obj.set_z_filter_max,
            self.pc_obj.get_z_filter_max, 0.2, 1.5,
            self.doublespinbox_cropbox_zmax,
            self.hslider_cropbox_zmax, 2)

    def create_voxel_grid_block(self):
        """Creates the Voxel Filter Block"""
        self.create_sub_component(
            "Leaf Size [m]", self.pc_obj.set_voxel_leaf_size,
            self.pc_obj.get_voxel_leaf_size, 0.001, 0.01,
            self.doublespinbox_voxelgrid_leafsize,
            self.hslider_voxelgrid_leafsize, 3)

    def create_seg_block(self):
        """Creates the Plane Segmentation Block"""
        self.create_sub_component(
            "Dist. Thresh [m]", self.pc_obj.set_plane_dist_thresh,
            self.pc_obj.get_plane_dist_thresh, 0.001, 0.05,
            self.doublespinbox_seg_thresh, self.hslider_seg_thresh, 3)
        self.create_sub_component(
            "Max Iterations", self.pc_obj.set_plane_max_iter,
            self.pc_obj.get_plane_max_iter, 25, 1000,
            self.spinbox_seg_iter, self.hslider_seg_iter, 0)

    def create_ror_block(self):
        """Creates the Radius Outlier Removal Block"""
        self.create_sub_component(
            "Min Neighbors", self.pc_obj.set_ror_min_neighbors,
            self.pc_obj.get_ror_min_neighbors, 1, 20, 
            self.spinbox_outlier_minneighbors,
            self.hslider_outlier_minneighbors, 0)
        self.create_sub_component(
            "Radius Search [m]", self.pc_obj.set_ror_radius_search,
            self.pc_obj.get_ror_radius_search, 0.005, 0.05,
            self.doublespinbox_outlier_radius,
            self.hslider_outlier_radius, 3)

    def create_cluster_block(self):
        """Creates the Cluster Filter Block"""
        self.create_sub_component(
            "Min Size", self.pc_obj.set_cluster_min_size,
            self.pc_obj.get_cluster_min_size, 25, 1000,
            self.spinbox_cluster_size_min,
            self.hslider_cluster_size_min, 0)
        self.create_sub_component(
            "Max Size", self.pc_obj.set_cluster_max_size,
            self.pc_obj.get_cluster_max_size, 25, 1000,
            self.spinbox_cluster_size_max,
            self.hslider_cluster_size_max, 0)
        self.create_sub_component(
            "Tolerance [m]", self.pc_obj.set_cluster_tol,
            self.pc_obj.get_cluster_tol, 0.01, 0.1,
            self.doublespinbox_cluster_tol,
            self.hslider_cluster_tol, 3)

    def create_button_block(self):
        """Creates a GUI subsection for the 'Load Configs', 'Save Configs', and 
        'Reset Configs' buttons        
        """
        self.button_config_reset.clicked.connect(self.reset_configs)
        self.button_config_load.clicked.connect(self.load_configs)
        self.button_config_save.clicked.connect(self.save_configs)

    def create_sub_component(
        self,
        name,
        set_func,
        get_func,
        min,
        max,
        display,
        slider,
        precision
    ):
        """Helper function used to create the Blocks above

        :param name: name of the Block
        :param set_func: function to set the value of a specific param
        :param get_func: function to get the value of a specfic param
        :param min: minimum value that the param can have
        :param max: maximum value that the param can have
        :param display: Ref to SpinBox or SpinBoxDouble Widget for this param
        :param slider: Ref to Slider Widget for this param
        :param precision: decimal precision of the param in 10**-precision
        """

        # Configure slider
        slider_range = int(round(abs(max - min)/10**-precision))
        slider_pctvalue = (get_func() - min) / float(max - min)
        slider.setRange(0, slider_range)
        slider.setValue(int(round(slider_pctvalue * slider_range)))

        # Configure display
        display.setValue(get_func())
        display.setRange(min, max)
        display.setSingleStep(10**-precision)
        if isinstance(display, QDoubleSpinBox):
            display.setDecimals(precision)

        # Configure signals
        display.valueChanged.connect(lambda:self.update_slider_bar(name))
        slider.valueChanged.connect(lambda:self.update_display(name))

        # global dictionary to store and retrieve values
        self.name_map[name] = {
            "display"   : display,
            "slider"    : slider,
            "min"       : min,
            "max"       : max,
            "set_func"  : set_func,
            "get_func"  : get_func,
            "range"     : slider_range,
            "precision" : precision
        }

        self.update_display(name)

# Event Handlers

    def update_slider_bar(self, name):
        """Event handler when a display is changed

        Updates the slider position to reflect that shown in the display

        :param name: name of the sub-component where the value was changed
        """
        info = self.name_map[name]
        value = float(info['display'].text())
        if (value < info['min']):
            value = info['min']
            info['display'].setValue(value)
        elif (value > info['max']):
            value = info['max']
            info['display'].setValue(value)
        pctvalue = (value - info['min']) / float(info['max'] - info['min'])
        info['slider'].setValue(int(round(pctvalue * info['range'])))

    def update_display(self, name):
        """Event handler when a slider is changed
        
        Updates the display to reflect the position dictated by the slider
        
        :param name: name of the sub-component where the value was changed
        """
        info = self.name_map[name]
        slider_value = info['slider'].value()
        pctvalue = slider_value / float(info['range'])
        value = pctvalue * (info['max'] - info['min']) + info['min']
        num = ('%.' + str(info['precision']) + 'f') % value
        # check if value is an integer or float
        if info['precision'] == 0:
            info['display'].setValue(int(num))
        else:
            info['display'].setValue(float(num))
        info['set_func'](value)

    def reset_configs(self):
        """Event handler when the 'Reset Configs' button is pressed
        
        Resets the displays and sliders to the values as defined in the loaded
            YAML file
        """
        self.pc_obj.load_params(self.filepath)
        for info in self.name_map.values():
            pctvalue = (info['get_func']() - info['min']) / float(info['max'] - info['min'])
            info['slider'].setValue(int(round(pctvalue * info['range'])))

    def load_configs(self):
        """Event handler when the 'Load Configs' button is pressed
        
        Opens up a dialogue box where the user can specify the YAML file to load
        """
        fname = QFileDialog.getOpenFileName(
            self,
            'Open file',
            self.filepath,
            "YAML files (*.yaml)"
        )
        if (fname[0] == ""): return
        self.filepath = fname[0]
        self.pc_obj.load_params(self.filepath)
        for info in self.name_map.values():
            pctvalue = (info['get_func']() - info['min']) / float(info['max'] - info['min'])
            info['slider'].setValue(int(round(pctvalue * info['range'])))

    def save_configs(self):
        """Event handler when the 'Save Configs' button is pressed
        
        Opens up a dialogue box where the user can specify the YAML file to
            which to save the parameters
        """
        fname = QFileDialog.getSaveFileName(
            self,
            'Save file',
            self.filepath,
            "YAML files (*.yaml)"
        )
        if (fname[0] == ""): return
        self.filepath = fname[0]
        self.pc_obj.save_params(self.filepath)

if __name__ == '__main__':
    rospy.init_node('pointcloud_tuner_gui')
    app = QApplication(sys.argv)
    gui = PointCloudTunerGui()
    # Only kill the program at node shutdown
    signal.signal(signal.SIGINT, signal.SIG_DFL)
    sys.exit(app.exec_())
